#include "pid.h"

void pid_init(short index, float P_, float I_, float D_, float Imin_, float Imax_)
{
  P[index] = P_;
  I[index] = I_;
  D[index] = D_;
  Isum[index] = 0;
  Imin[index] = Imin_;
  Imax[index] = Imax_;
  last_error[index] = 0;
}

float pid_update(short index, float error)
{
  Isum[index] += I[index] * error;
  if (Isum[index] > Imax[index])
    Isum[index] = Imax[index];
  if (Isum[index] < Imin[index])
    Isum[index] = Imin[index];
  
  float Result = P[index] * error + Isum[index] + D[index] * (error - last_error[index]);
  
  last_error[index] = error;
  
  return Result;
}
